##!/usr/bin/python
import sys
sys.path.insert(0, '/home/rosdell/wiibalance/cwiid/python/build/lib.linux-x86_64-2.7/')
sys.path.insert(0, '/home/rosdell/wiibalance/cwiid/libcwiid/')
import cwiid
import sys
import time

def main():
	#Connect to address given on command-line, if present
	print 'Put Wiimote in discoverable mode now (press 1+2)...'
	global wiimote
	if len(sys.argv) > 1:
		wiimote = cwiid.Wiimote(sys.argv[1])
	else:
		wiimote = cwiid.Wiimote()
	wiimote.rpt_mode = cwiid.RPT_BALANCE | cwiid.RPT_BTN
	wiimote.request_status()
	balance_calibration = wiimote.get_balance_cal()
	named_calibration = { 'right_top': balance_calibration[0],
						  'right_bottom': balance_calibration[1],
						  'left_top': balance_calibration[2],
						  'left_bottom': balance_calibration[3],
						}
	exit = False
	while not exit:
		wiimote.request_status()
		readss = wiimote.state['balance']
		wiibalance_raw = readss.values()
		time.sleep(0.1)
		wiibalance_weight = calcweight(readss, named_calibration)
		print wiibalance_weight, wiibalance_raw
	return 0

def calcweight( readings, calibrations ):
	"""
	Determine the weight of the user on the board in hundredths of a kilogram
	"""
	weight = [0,0,0,0]
	i = 0
	for sensor in ('left_bottom', 'right_top', 'right_bottom', 'left_top'):
		reading = readings[sensor]
		calibration = calibrations[sensor]
		if reading < calibration[1]:
			weight[i] = (1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]))/100.0
		else:
			weight[i] = (1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700)/100.0
		i+=1
	return weight
	
if __name__ == "__main__":
	sys.exit(main())
